#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include "Classifier.h"
#include <iostream>
#include <fstream>
#include <drawstuff/drawstuff.h>
#include <string>
#include <cstdlib>
#include <ctime>
#include "main.h"
#include <utility>
#include <stack>
#include <stdio.h>
#include <time.h>

#define GOALX 2.75
#define GOALY 0.00
#define d 0.01


using namespace std;

struct step { //the instructions necessary for a single robot step
  public:
     int foot;
     float x;
     float y;

     step(Node* targetNode){
        foot = targetNode->moving_foot;
        x = targetNode->foot_pos[foot].n[0];
        y = targetNode->foot_pos[foot].n[1];
     }
     
};

class smallest_f // comparison function for priority queue
{
public:
  bool operator() (Node* lhs, Node* rhs) const
  {
    return (lhs->f > rhs->f);
  }
};

class smallest_node // comparison function for multiset
{
public:
  bool operator() (Node* lhs, Node* rhs) const
  {
     float gridSize = d/2; //Grid size used for determining same states.
     int Xl = floor(lhs->center[0]/gridSize);
     int Yl = floor(lhs->center[1]/gridSize);
     int Xr = floor(rhs->center[0]/gridSize);
     int Yr = floor(rhs->center[1]/gridSize);
     bool answer = (Xl < Xr) || ((Xr == Xl) && (Yl < Yr));
     return answer;
  }
};
 
//This Method takes x and y coordinates and returns the Eucidian distance
float EuclidDist(float deltaX, float deltaY) { //Calculates the Euclidian Distance of an ordered pair
        return sqrt(deltaX*deltaX + deltaY*deltaY);
};

//Calculates the Euclidian Distance of a bduVec3f from the origin
float ThreeEuclidDist(bduVec3f *dist) { 
        return sqrt(dist->n[0]*dist->n[0] + dist->n[1]*dist->n[1] + dist->n[2]*dist->n[2]);
}

// calculates the euclidian distance between two points
float CoordEucDist(bduVec3f *vec1, bduVec3f *vec2){
	return sqrt( (vec1->n[0] - vec2->n[0])*(vec1->n[0] - vec2->n[0]) + (vec1->n[1] - vec2->n[1])*(vec1->n[1] - vec2->n[1]) + (vec1->n[2] - vec2->n[2])*(vec1->n[2] - vec2->n[2]) );
}

//This method takes a node pointer and tests whether it corresponds to a goal state
bool GoalTest(Node * testNode) {
     return EuclidDist(testNode->center[0] - GOALX,testNode->center[1]-GOALY) < d;
}

//This method recursively backtracks the A* search tree from a goal-state node to populate
//the stack<step>, storing the dog's path.
void FindPath(stack<step>* path, Node* currentNode){ //Recursively backtracks the A* search path
      if(currentNode->parent != NULL){
         path->push(step(currentNode));
         FindPath(path, currentNode->parent);
      }
}

//Returns the mean of the altitude of 3 foot configuration
double altitudeMean(bduVec3f* v1, bduVec3f* v2, bduVec3f* v3){
    double sum = v1->n[2] + v3->n[2] + v3->n[2];
    return sum/3;
} 


//This method takes a struct<step> pointer representing a path solotion for the dog and
//prints this path to a file. 
void SavePathToFile(stack<step> * path){
     ofstream pathFile("milestone.path");
     printf("Printing path file to milestone.path\n");
     if(!pathFile){
        printf("Error opening the file!\n");
     }
     while(!path->empty()){
        step nextStep = path->top();
        path->pop();
        pathFile << nextStep.foot << " " << nextStep.x << " " << nextStep.y << endl;
     }
}

//This method prints a node to the terminal - generally used for debugging
void PrintFootPositions(Node* node){
     //printf("\nf: %f\n", node->f);
     bduVec3f* globalFeet = node->foot_pos;
     printf("%d, (%f, %f, %f) (%f, %f, %f) (%f, %f, %f) (%f, %f, %f)\n", node, globalFeet[0].n[0],globalFeet[0].n[1],globalFeet[0].n[2],globalFeet[1].n[0], globalFeet[1].n[1],globalFeet[1].n[2],globalFeet[2].n[0],globalFeet[2].n[1],globalFeet[2].n[2],globalFeet[3].n[0],globalFeet[3].n[1],globalFeet[3].n[2]);
}

//This method calculates and returns a pointer to the center of a node (configuration of dog feet)
float* FindNodeCenter(Node* node){
     float* center = new float[2];
     center[0] = 0;
     center[1] = 0;
     for(int i = 0; i < 4; i++){
        center[0] += node->foot_pos[i].n[0];
        center[1] += node->foot_pos[i].n[1];
     }
     center[0] /= 4;
     center[1] /= 4;
     return center;
}

//This method Generates a child node and returns it only if the leg Classifiers validate it, else returns NULL
Node* GenerateChildNode(SDSimulator* simulator, Classifier *cl0, Classifier *cl1, Classifier *cl2, Classifier *cl3, Node* current,int i,int j){
 Datum data;
     data.num_features = 16;
     double points[16];
     float sumx, sumy;
     data.features = points;
     bduVec3f localFeet[4];
     bduVec3f localStep;
     simulator->setState(current->state);
     simulator->getLocalFootPositions(localFeet);
     simulator->getLocalStepPosition(current->foot_pos[current->moving_foot].n[0] + i*d, current->foot_pos[current->moving_foot].n[1] + j*d, &localStep);
     points[0] = 1;

// compute the features for the classifiers, and store then in the array "points"
     if(current->moving_foot == 0){
      // distances to center of dog  (4 features)
       points[1] = ThreeEuclidDist(&localStep);
       points[2] = ThreeEuclidDist(&localFeet[1]);
       points[3] = ThreeEuclidDist(&localFeet[2]);
       points[4] = ThreeEuclidDist(&localFeet[3]);

      //Distance of moving foot to other 3 feet (3 features)
       points[5] = CoordEucDist(&localStep, &localFeet[1]);
       points[6] = CoordEucDist(&localStep, &localFeet[3]);
       points[7] = CoordEucDist(&localStep, &localFeet[2]);

       //min distance to other feet (1 feature)
       points[8] = min(CoordEucDist(&localStep, &localFeet[2]),min(CoordEucDist(&localStep, &localFeet[1]),CoordEucDist(&localStep, &localFeet[3])));
       //max distance to other feet (1 feature)
       points[9] = max(CoordEucDist(&localStep, &localFeet[2]),max(CoordEucDist(&localStep, &localFeet[1]),CoordEucDist(&localStep, &localFeet[3])));
       //distance of moving_foot to mean of other 3 feet (1 feature)
        sumx = (localFeet[1].n[0]+localFeet[2].n[0]+localFeet[3].n[0]);
        sumy = (localFeet[1].n[1]+localFeet[2].n[1]+localFeet[3].n[1]);
       points[10] = sqrt( pow(sumx/3-localStep.n[0],2) + pow(sumy/3-localStep.n[1],2));

       //coordinates mean of 4 feet (2 features)
       sumx = (sumx + localStep.n[0])/4;
       sumy = (sumy + localStep.n[1])/4 ;
       points[11] = sumx;
       points[12] = sumy;

      //distance of each foot to mean of 4 feet (4 features) 
       points[13] = sqrt( pow(sumx-localStep.n[0],2) + pow(sumy-localStep.n[1],2));
       points[14] = sqrt( pow(sumx-localFeet[1].n[0],2) + pow(sumy-localFeet[1].n[1],2));
       points[15] = sqrt( pow(sumx-localFeet[2].n[0],2) + pow(sumy-localFeet[2].n[1],2));
       points[16] = sqrt( pow(sumx-localFeet[3].n[0],2) + pow(sumy-localFeet[3].n[1],2));

       //altitude distance (1 feature)
       points[17] = pow(altitudeMean(&localFeet[1], &localFeet[2], &localFeet[3])-localStep.n[2] , 2);

       //ratio of diagonals of the polygon formed by the feet's positions
       points[18] = CoordEucDist(&localStep, &localFeet[3])/CoordEucDist(&localFeet[1], &localFeet[2]);

	    if(cl0->getClassOf(data) == 1){
	        printf("Foot 0 : step classified as impossible\n");
	        return NULL;
	}
     }
     else if(current->moving_foot == 1){
	//get distances to center of dog  4
       points[1] = ThreeEuclidDist(&localStep);
       points[2] = ThreeEuclidDist(&localFeet[0]);
       points[3] = ThreeEuclidDist(&localFeet[2]);
       points[4] = ThreeEuclidDist(&localFeet[3]);

      //Distance of moving foot to other 3 feet
       points[5] = CoordEucDist(&localStep, &localFeet[3]);
       points[6] = CoordEucDist(&localStep, &localFeet[2]);
       points[7] = CoordEucDist(&localStep, &localFeet[0]);

       //min distance to other feet
       points[8] = min(CoordEucDist(&localStep, &localFeet[2]),min(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[3])));
       //max distance to other feet
       points[9] = max(CoordEucDist(&localStep, &localFeet[2]),max(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[3])));
       //distance of moving_foot to mean of other 3 feet
        sumx = (localFeet[0].n[0]+localFeet[2].n[0]+localFeet[3].n[0]);
        sumy = (localFeet[0].n[1]+localFeet[2].n[1]+localFeet[3].n[1]);
       points[10] = sqrt( pow(sumx/3-localStep.n[0],2) + pow(sumy/3-localStep.n[1],2));

       //mean of 4 feet
       sumx = (sumx + localStep.n[0])/4;
       sumy = (sumy + localStep.n[1])/4 ;
       points[11] = sumx;
       points[12] = sumy;

      //distance of each foot to mean of 4 feet 
       points[13] = sqrt( pow(sumx-localStep.n[0],2) + pow(sumy-localStep.n[1],2));
       points[14] = sqrt( pow(sumx-localFeet[0].n[0],2) + pow(sumy-localFeet[0].n[1],2));
       points[15] = sqrt( pow(sumx-localFeet[2].n[0],2) + pow(sumy-localFeet[2].n[1],2));
       points[16] = sqrt( pow(sumx-localFeet[3].n[0],2) + pow(sumy-localFeet[3].n[1],2));

       //altitude distance
       points[17] = pow(altitudeMean(&localFeet[0], &localFeet[2], &localFeet[3])-localStep.n[2] , 2);

       //ratio of diagonals
       points[18] = CoordEucDist(&localStep, &localFeet[2])/CoordEucDist(&localFeet[0], &localFeet[3]);

	    if(cl1->getClassOf(data) == 1){
	        printf("Foot 1 : step classified as impossible\n");
	        return NULL;
	}
     }
     else if(current->moving_foot == 2){
	//get distances to center of dog  4
       points[1] = ThreeEuclidDist(&localStep);
       points[2] = ThreeEuclidDist(&localFeet[0]);
       points[3] = ThreeEuclidDist(&localFeet[1]);
       points[4] = ThreeEuclidDist(&localFeet[3]);

      //Distance of moving foot to other 3 feet
       points[5] = CoordEucDist(&localStep, &localFeet[0]);
       points[6] = CoordEucDist(&localStep, &localFeet[1]);
       points[7] = CoordEucDist(&localStep, &localFeet[3]);

       //min distance to other feet
       points[8] = min(CoordEucDist(&localStep, &localFeet[1]),min(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[3])));
       //max distance to other feet
       points[9] = max(CoordEucDist(&localStep, &localFeet[1]),max(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[3])));
       //distance of moving_foot to mean of other 3 feet
        sumx = (localFeet[0].n[0]+localFeet[1].n[0]+localFeet[3].n[0]);
        sumy = (localFeet[0].n[1]+localFeet[1].n[1]+localFeet[3].n[1]);
       points[10] = sqrt( pow(sumx/3-localStep.n[0],2) + pow(sumy/3-localStep.n[1],2));

       //mean of 4 feet
       sumx = (sumx + localStep.n[0])/4;
       sumy = (sumy + localStep.n[1])/4 ;
       points[11] = sumx;
       points[12] = sumy;

      //distance of each foot to mean of 4 feet 
       points[13] = sqrt( pow(sumx-localStep.n[0],2) + pow(sumy-localStep.n[1],2));
       points[14] = sqrt( pow(sumx-localFeet[0].n[0],2) + pow(sumy-localFeet[0].n[1],2));
       points[15] = sqrt( pow(sumx-localFeet[1].n[0],2) + pow(sumy-localFeet[1].n[1],2));
       points[16] = sqrt( pow(sumx-localFeet[3].n[0],2) + pow(sumy-localFeet[3].n[1],2));

       //altitude distance
       points[17] = pow(altitudeMean(&localFeet[0], &localFeet[1], &localFeet[3])-localStep.n[2] , 2);

       //ratio of diagonals
       points[18] = CoordEucDist(&localStep, &localFeet[1])/CoordEucDist(&localFeet[0], &localFeet[3]);

	    if(cl2->getClassOf(data) == 1){
	        printf("Foot 2 : step classified as impossible\n");
	        return NULL;
	}
     }
     else if(current->moving_foot == 3){
	//get distances to center of dog  4
       points[1] = ThreeEuclidDist(&localStep);
       points[2] = ThreeEuclidDist(&localFeet[1]);
       points[3] = ThreeEuclidDist(&localFeet[2]);
       points[4] = ThreeEuclidDist(&localFeet[0]);

      //Distance of moving foot to other 3 feet
       points[5] = CoordEucDist(&localStep, &localFeet[2]);
       points[6] = CoordEucDist(&localStep, &localFeet[0]);
       points[7] = CoordEucDist(&localStep, &localFeet[1]);

       //min distance to other feet
       points[8] = min(CoordEucDist(&localStep, &localFeet[2]),min(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[1])));
       //max distance to other feet
       points[9] = max(CoordEucDist(&localStep, &localFeet[2]),max(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[1])));
       //distance of moving_foot to mean of other 3 feet
        sumx = (localFeet[0].n[0]+localFeet[2].n[0]+localFeet[1].n[0]);
        sumy = (localFeet[0].n[1]+localFeet[2].n[1]+localFeet[1].n[1]);
       points[10] = sqrt( pow(sumx/3-localStep.n[0],2) + pow(sumy/3-localStep.n[1],2));

       //mean of 4 feet
       sumx = (sumx + localStep.n[0])/4;
       sumy = (sumy + localStep.n[1])/4 ;
       points[11] = sumx;
       points[12] = sumy;

      //distance of each foot to mean of 4 feet 
       points[13] = sqrt( pow(sumx-localStep.n[0],2) + pow(sumy-localStep.n[1],2));
       points[14] = sqrt( pow(sumx-localFeet[0].n[0],2) + pow(sumy-localFeet[0].n[1],2));
       points[15] = sqrt( pow(sumx-localFeet[1].n[0],2) + pow(sumy-localFeet[1].n[1],2));
       points[16] = sqrt( pow(sumx-localFeet[2].n[0],2) + pow(sumy-localFeet[2].n[1],2));

       //altitude distance
       points[17] = pow(altitudeMean(&localFeet[0], &localFeet[1], &localFeet[2])-localStep.n[2] , 2);

       //ratio of diagonals
       points[18] = CoordEucDist(&localStep, &localFeet[0])/CoordEucDist(&localFeet[2], &localFeet[1]);

	    if(cl3->getClassOf(data) == 1){
	        printf("Foot 3 : step classified as impossible\n");
	        return NULL;
     	}
     }


// create the child node
     float gridSize = 4*d; //This defines the step size to be used
     Node* child = new Node();
     child->foot_pos = new bduVec3f[4];
     for(int k = 0; k < 4; k++){
        if(current->moving_foot != k){
           child->foot_pos[k] = current->foot_pos[k];
        }else{
           float x = current->foot_pos[k].n[0] + i*gridSize;
           float y = current->foot_pos[k].n[1] + j*gridSize;
           child->foot_pos[k].n[0] = x;
           child->foot_pos[k].n[1] = y;
           child->foot_pos[k].n[2] = simulator->getPointHeight(x,y);
        }
     }

     child->moving_foot = (current->moving_foot + 1)%4;
     child->center = FindNodeCenter(child);
     child->h = 4*sqrt(pow(child->center[0]-GOALX,2) + pow(child->center[1]-GOALY,2))/(sqrt(8)*gridSize);
     child->g = current->g + 1;
     child->f = child->g + child->h;
     child->parent = current;
     return child;
}

//This method calculates the distance between the indicated feet of the indicated node (dog configuration) in the x-y plain
float FootDist(Node* node, int foot1, int foot2){
     return sqrt(pow(node->foot_pos[foot1].n[0] - node->foot_pos[foot2].n[0],2) + pow(node->foot_pos[foot1].n[1] - node->foot_pos[foot2].n[1],2));
  }
  
//This method takes a reference to a node being considered for expansion and determines whether
//the step is possible - first by checking basic invalid conditions and second by simulation
bool ValidStep(Node* node, SDSimulator* simulator, SDSlowWalk& walkController, int* number, SDLittleDog& littleDog){
  // check for start node
     if(node->parent == NULL){
        printf("validating start node.\n");
        return true;
     }
 
  // check if the step brings two feet too close together
     for(int i = 0; i < 4; i++){
        for(int j = i+1; j < 4; j++){
           if(FootDist(node, i, j) < .05){
		printf("feet too close! trimming\n");
                (*number)++;
		return false;
	   }
        }
     } 

  // check if the moving foot is too high compared to the other 3
     int foot = node->moving_foot;
     float altmean = (node->foot_pos[(foot+1)%  4].n[2] + node->foot_pos[(foot+2)%  4].n[2] + node->foot_pos[(foot+3)%  4].n[2])/3;
     float altstep = fabs(altmean - node->foot_pos[foot].n[2]);
     printf("\nAlt mean : %f , Alt step : %f\n", altmean,altstep );
     if ( altstep > 0.05 ){
         printf(" !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!trimming node, alstep = %f : on a terrain bump\n", altstep);
         printf("Altitudes : %f %f %f   and  %f\n",node->foot_pos[(foot+1)%  4].n[2], node->foot_pos[(foot+2)%  4].n[2], node->foot_pos[(foot+3)%  4].n[2], node->foot_pos[foot].n[2] );
         (*number)++;
         getchar();
         return false;
     }

  // Simulate step
     SDSimulatorState *oldState = node->parent->state; 
     simulator->setState(oldState);
     step stepParam = step(node);
     walkController.setStepPosition(stepParam.foot, stepParam.x, stepParam.y);
     littleDog.runTrial(oldState);
     bduVec3f globalFeet[4];
     simulator->getGlobalFootPositions(globalFeet);
     float x = stepParam.x;
     float y = stepParam.y;
     float zoff = 0;
	for(int z=0; z<4; z++){ //this loop checks if the dog has flipped
		zoff += fabs(globalFeet[z].n[2] - simulator->getPointHeight(globalFeet[z].n[0], globalFeet[z].n[1]));
  	}
        if( zoff > 0.2 ){
		printf("Oh my dog it flipped!\n");
                (*number)++;
	   return false;
	}
// check if the feet have slipped
        else if((x+ .05 > globalFeet[node->moving_foot].n[0]) && (x- .05 < globalFeet[node->moving_foot].n[0]) && (y+.05 > globalFeet[node->moving_foot].n[1]) && (y-.05 < globalFeet[node->moving_foot].n[1])){
            SDSimulatorState* state = new SDSimulatorState();
            simulator->getState(state);
            node->state = state;
		printf("(%f, %f, %f, %f -- %f, %f, %f, %f )- good one\n", globalFeet[0].n[0], globalFeet[1].n[0], globalFeet[2].n[0], globalFeet[3].n[0], globalFeet[0].n[1], globalFeet[1].n[1], globalFeet[2].n[1], globalFeet[3].n[1]);
            return true;
        } 
        else{
            (*number)++;
		printf("bad one\n");
            return false;
	}
    (*number)++;
    return false;
}


//This function makes the classifier and puts all of the data in the 4 files opened by ofstream below
//It uses the simulation to tell what moves are valid and enters relevant data into the files
//the numExamples parameter used with the classifier of the file is the number of iterations of the outer for loop,
//and the num_feat parameter is the number of elements put into the out ofstreams
bool makeFootPosClassifier(SDSimulator *simulator, SDLittleDog& littleDog, SDSlowWalk& walkController, int num_examples, int num_feat){

  ofstream out0("classifier-data-dist0.txt");
  if(!out0){
    printf("Error opening the file!\n");
    return false;
  }
  ofstream out1("classifier-data-dist1.txt");
  if(!out1){
    printf("Error opening the file!\n");
    return false;
  } 
  ofstream out2("classifier-data-dist2.txt");
  if(!out2){
    printf("Error opening the file!\n");
    return false;
  }
  ofstream out3("classifier-data-dist3.txt");
  if(!out3){
    printf("Error opening the file!\n");
    return false;
  }
 
  printf("Getting data for footPos classifier....\n"); 
  srand((unsigned)time(0));
  bduVec3f globalFeet[4];
  bduVec3f localStep;
  bduVec3f localFeet[4];
  float x,y,zoff;
  int result;
  SDSimulatorState state, initState;

  simulator->getState(&initState); //keeps initState as the initial state
  
 //print number of features in file
  out0 << num_feat << endl;
  out1 << num_feat << endl;
  out2 << num_feat << endl;
  out3 << num_feat << endl;

// generate random steps
  for(int i=0; i< num_examples; i++){
    for (int j=0; j<4; j++){
       simulator->getState(&state);
       simulator->getLocalFootPositions(localFeet);

       //make a random move with foot j
       //to its current configuration + (x,y)
       x = double(rand()%1001 - 500)/5000;
       y = double(rand()%1001 - 500)/5000;
       simulator->getGlobalFootPositions(globalFeet);
	
       x += globalFeet[j].n[0];
       y += globalFeet[j].n[1];

       simulator->getLocalStepPosition(x,y,&localStep);
       walkController.setStepPosition(j, x, y);
       littleDog.runTrial(&state);

       //check if the actual configuration is within 1cm 
       //of the intended configuration (x,y)
        simulator->getState(&state);
        simulator->setState(&state);  // This prevents the simulator straying from search
        simulator->getGlobalFootPositions(globalFeet);
	zoff = 0;
	for(int z=0; z<4; z++){ //this loop checks if the dog has flipped
		zoff += fabs(globalFeet[z].n[2] - simulator->getPointHeight(globalFeet[z].n[0], globalFeet[z].n[1]));
	}

	if( zoff > 0.2 ){
	   result = 1;
	   printf("The dog has flipped!\n");
	   simulator->setState(&initState);
	}
        else if((x+ .01 > globalFeet[j].n[0]) && (x- .01 < globalFeet[j].n[0]) && (y+.01 > globalFeet[j].n[1]) && (y-.01 < globalFeet[j].n[1])){
            result = 0;
        } 
        else{
            result = 1;
	}

  //write the features to the 4 data files for the classifiers
	float sumx, sumy;
    	if(j==0){
       	    out0 << "1 ";
       /*
//position of feet (12 feat)
       out0 << localStep.n[0] << " " << localStep.n[1] << " " << localStep.n[2] << " ";
       out0 << localFeet[1].n[0] << " " << localFeet[1].n[1] << " " << localFeet[1].n[2] << " ";
       out0 << localFeet[2].n[0] << " " << localFeet[2].n[1] << " " << localFeet[2].n[2] << " ";
      out0 << localFeet[3].n[0] << " " << localFeet[3].n[1] << " " << localFeet[3].n[2] << " ";
*/
	//get distances to center of dog  4
       out0 << ThreeEuclidDist(&localStep) << " " << ThreeEuclidDist(&localFeet[1]) << " " << ThreeEuclidDist(&localFeet[2]) << " " << ThreeEuclidDist(&localFeet[3]) << " ";

       //distance of moving foot to other feet (3 feat)
       out0 << CoordEucDist(&localStep, &localFeet[1]) << " " << CoordEucDist(&localStep, &localFeet[3]) << " " << CoordEucDist(&localStep, &localFeet[2]) << " " ;
//min distance to other feet (1 feet)
       out0 << min(CoordEucDist(&localStep, &localFeet[2]),min(CoordEucDist(&localStep, &localFeet[1]),CoordEucDist(&localStep, &localFeet[3]))) << " ";
//max distance to other feet (1 feet)
       out0 << max(CoordEucDist(&localStep, &localFeet[2]),max(CoordEucDist(&localStep, &localFeet[1]),CoordEucDist(&localStep, &localFeet[3]))) << " ";

// distance of moving_foot to mean of other 3 feet (1 feat)
       sumx = (localFeet[1].n[0]+localFeet[2].n[0]+localFeet[3].n[0]);
       sumy = (localFeet[1].n[1]+localFeet[2].n[1]+localFeet[3].n[1]);
      out0 << sqrt( pow(sumx/3-localStep.n[0],2) + pow(sumy/3-localStep.n[1],2)) << " ";

// mean of 4 feet (2 feat)
       sumx = (sumx + localStep.n[0])/4;
       sumy = (sumy + localStep.n[1])/4 ;
      out0 << sumx << " "<< sumy  << " "; 

// distance of each foot to mean of 4 feet (4 feat)
      out0 << sqrt( pow(sumx-localStep.n[0],2) + pow(sumy-localStep.n[1],2)) << " ";
      out0 << sqrt( pow(sumx-localFeet[1].n[0],2) + pow(sumy-localFeet[1].n[1],2)) << " " ;
      out0 << sqrt( pow(sumx-localFeet[2].n[0],2) + pow(sumy-localFeet[2].n[1],2))<< " ";
      out0 << sqrt( pow(sumx-localFeet[3].n[0],2) + pow(sumy-localFeet[3].n[1],2)) << " ";

// altitude distance, squared (1 feat)
       out0 << pow(altitudeMean(&localFeet[1], &localFeet[2], &localFeet[3])-localStep.n[2] , 2) <<" ";

// ratio of diagonals (1 feat)
       out0 << CoordEucDist(&localStep, &localFeet[3])/CoordEucDist(&localFeet[1], &localFeet[2]) << " ";
       out0 << result << endl;
    }
    else if (j==1){
       out1 << "1 ";
       /*
      out1 << localFeet[0].n[0] << " " << localFeet[0].n[1] << " " << localFeet[0].n[2] << " ";
       out1 << localStep.n[0] << " " << localStep.n[1] << " " << localStep.n[2] << " ";
       out1 << localFeet[2].n[0] << " " << localFeet[2].n[1] << " " << localFeet[2].n[2] << " ";
       out1 << localFeet[3].n[0] << " " << localFeet[3].n[1] << " " << localFeet[3].n[2] << " ";
  */
       
	//get distances to center of dog  4
       out1 << ThreeEuclidDist(&localStep) << " " << ThreeEuclidDist(&localFeet[0]) << " " << ThreeEuclidDist(&localFeet[2]) << " " << ThreeEuclidDist(&localFeet[3]) << " ";

      //Distance of moving foot to other 3 feet
       out1 << CoordEucDist(&localStep, &localFeet[3]) << " " << CoordEucDist(&localStep, &localFeet[2]) << " " << CoordEucDist(&localStep, &localFeet[0]) << " " ;

       //min distance to other feet
       out1 << min(CoordEucDist(&localStep, &localFeet[2]),min(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[3]))) << " ";
       //max distance to other feet
       out1 << max(CoordEucDist(&localStep, &localFeet[2]),max(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[3]))) << " ";
       //distance of moving_foot to mean of other 3 feet
        sumx = (localFeet[0].n[0]+localFeet[2].n[0]+localFeet[3].n[0]);
        sumy = (localFeet[0].n[1]+localFeet[2].n[1]+localFeet[3].n[1]);
       out1 << sqrt( pow(sumx/3-localStep.n[0],2) + pow(sumy/3-localStep.n[1],2)) << " ";

       //mean of 4 feet
       sumx = (sumx + localStep.n[0])/4;
       sumy = (sumy + localStep.n[1])/4 ;
       out1 << sumx << " "<< sumy  << " ";

      //distance of each foot to mean of 4 feet 
       out1 << sqrt( pow(sumx-localStep.n[0],2) + pow(sumy-localStep.n[1],2)) << " ";
       out1 << sqrt( pow(sumx-localFeet[0].n[0],2) + pow(sumy-localFeet[0].n[1],2)) << " " ;
       out1 << sqrt( pow(sumx-localFeet[2].n[0],2) + pow(sumy-localFeet[2].n[1],2)) << " " ;
       out1 << sqrt( pow(sumx-localFeet[3].n[0],2) + pow(sumy-localFeet[3].n[1],2)) << " ";

       //altitude distance
       out1 << pow(altitudeMean(&localFeet[0], &localFeet[2], &localFeet[3])-localStep.n[2] , 2) <<" ";

       //ratio of diagonals
       out1 << CoordEucDist(&localStep, &localFeet[2])/CoordEucDist(&localFeet[0], &localFeet[3]) << " ";
       out1 << result << endl;
    }
    else if (j==2){
       out2 << "1 ";
       /*
       out2 << localFeet[0].n[0] << " " << localFeet[0].n[1] << " " << localFeet[0].n[2] << " ";
       out2 << localFeet[1].n[0] << " " << localFeet[1].n[1] << " " << localFeet[1].n[2] << " ";
       out2 << localStep.n[0] << " " << localStep.n[1] << " " << localStep.n[2] << " ";
       out2 << localFeet[3].n[0] << " " << localFeet[3].n[1] << " " << localFeet[3].n[2] << " ";
     */

	//get distances to center of dog  4
       out2 << ThreeEuclidDist(&localStep) << " " << ThreeEuclidDist(&localFeet[0]) << " " << ThreeEuclidDist(&localFeet[1]) << " " << ThreeEuclidDist(&localFeet[3]) << " ";

       //distance of moving feet to other 3 feet
       out2 << CoordEucDist(&localStep, &localFeet[0]) << " " << CoordEucDist(&localStep, &localFeet[1]) << " " << CoordEucDist(&localStep, &localFeet[3]) << " " ;
       // min distance to other feet
       out2 << min(CoordEucDist(&localStep, &localFeet[1]),min(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[3]))) << " ";
       // max distance to other feet
       out2 << max(CoordEucDist(&localStep, &localFeet[1]),max(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[3]))) << " ";
       // distance of moving_foot to mean of other 3 feet
        sumx = (localFeet[0].n[0]+localFeet[1].n[0]+localFeet[3].n[0]);
        sumy = (localFeet[0].n[1]+localFeet[1].n[1]+localFeet[3].n[1]);
       out2 << sqrt( pow(sumx/3-localStep.n[0],2) + pow(sumy/3-localStep.n[1],2)) << " ";
       // mean of 4 feet
        sumx = (sumx + localStep.n[0])/4;
        sumy = (sumy + localStep.n[1])/4 ;
       out2 << sumx << " "<< sumy  << " "; 

       //distance of each foot to mean of 4 feet
      out2 << sqrt( pow(sumx-localStep.n[0],2) + pow(sumy-localStep.n[1],2)) << " " ;
      out2 << sqrt( pow(sumx-localFeet[0].n[0],2) + pow(sumy-localFeet[0].n[1],2)) << " " ;
      out2 << sqrt( pow(sumx-localFeet[1].n[0],2) + pow(sumy-localFeet[1].n[1],2))<< " " ;
      out2 << sqrt( pow(sumx-localFeet[3].n[0],2) + pow(sumy-localFeet[3].n[1],2)) << " ";

      //altitude distance
       out2 << pow(altitudeMean(&localFeet[0], &localFeet[1], &localFeet[3])-localStep.n[2] , 2) <<" ";
       //ratio of diagonals
       out2 << CoordEucDist(&localStep, &localFeet[1])/CoordEucDist(&localFeet[0], &localFeet[3]) << " ";
       out2 << result << endl;
    }
    else if (j==3){
       out3 << "1 ";
       /*
     out3 << localFeet[0].n[0] << " " << localFeet[0].n[1] << " " << localFeet[0].n[2] << " ";
       out3 << localFeet[1].n[0] << " " << localFeet[1].n[1] << " " << localFeet[1].n[2] << " ";
       out3 << localFeet[2].n[0] << " " << localFeet[2].n[1] << " " << localFeet[2].n[2] << " ";
       out3 << localStep.n[0] << " " << localStep.n[1] << " " << localStep.n[2] << " ";
      */

	//get distances to center of dog  4
       out3 << ThreeEuclidDist(&localStep) << " " << ThreeEuclidDist(&localFeet[1]) << " " << ThreeEuclidDist(&localFeet[2]) << " " << ThreeEuclidDist(&localFeet[0]) << " ";

       //distance of moving feet to other 3 feet
       out3 << CoordEucDist(&localStep, &localFeet[2]) << " " << CoordEucDist(&localStep, &localFeet[0]) << " " << CoordEucDist(&localStep, &localFeet[1]) << " " ;
       // min distance to other feet
       out3 << min(CoordEucDist(&localStep, &localFeet[2]),min(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[1]))) << " ";
       // max distance to other feet
       out3 << max(CoordEucDist(&localStep, &localFeet[2]),max(CoordEucDist(&localStep, &localFeet[0]),CoordEucDist(&localStep, &localFeet[1]))) << " ";

       // distance of moving_foot to mean of other 3 feet
        sumx = (localFeet[0].n[0]+localFeet[2].n[0]+localFeet[1].n[0]);
        sumy = (localFeet[0].n[1]+localFeet[2].n[1]+localFeet[1].n[1]);
       out3 << sqrt( pow(sumx/3-localStep.n[0],2) + pow(sumy/3-localStep.n[1],2)) << " ";

       // mean of 4 feet
        sumx = (sumx + localStep.n[0])/4;
        sumy = (sumy + localStep.n[1])/4 ;
       out3 << sumx << " "<< sumy  << " ";  

       //distance of each foot to mean of 4 feet
      out3 << sqrt( pow(sumx-localStep.n[0],2) + pow(sumy-localStep.n[1],2)) << " " ;
      out3 << sqrt( pow(sumx-localFeet[0].n[0],2) + pow(sumy-localFeet[0].n[1],2)) << " " ;
      out3 << sqrt( pow(sumx-localFeet[1].n[0],2) + pow(sumy-localFeet[1].n[1],2))<< " " ;
      out3 << sqrt( pow(sumx-localFeet[2].n[0],2) + pow(sumy-localFeet[2].n[1],2)) << " ";

      //altitude distance
       out3 << pow(altitudeMean(&localFeet[0], &localFeet[1], &localFeet[2])-localStep.n[2] , 2) <<" ";
       //ratio of diagonals
       out3 << CoordEucDist(&localStep, &localFeet[0])/CoordEucDist(&localFeet[2], &localFeet[1]) << " ";
       out3 << result << endl;
	}	


    }
  }
  return true;
}


/**
 * Here is where all the magic happens.  With the 
 * default setup, the -path flag will load "actions.path"
 * and attempt to simulate the footsteps line by line.
 * This is for us to see the robot walk along your path.
 * We have provided an example file to make the robot walk
 * .2 meters ahead.  Note that the simulation starts off
 * paused.  You can use Ctrl+P to move it ahead one step at
 * a time, or you can change the "pause" argument in 
 * simulator/LittleDog.cpp to be something else.  To see
 * the terrain press 't' (note the terrain is simulated
 * even if it is not displayed graphically).
 * 
 * Your job is to write the code that will generate
 * the path file - that is you must discretize the search
 * space, perform A* search using a logistic regression
 * classifier (trained before hand or on the fly), and
 * finally write the returned path to file.  Once the 
 * milestone is submitted you may change virtually anything
 * you want to improve performance, including the controller
 * used.  When you submit, you will tell us the flags your
 * program needs to be run with in order to get the desired
 * behavior.
 */

bool runPath(string file, SDSimulator *simulator, SDLittleDog& littleDog,
	     SDSlowWalk& walkController){
  string line;
  ifstream paths;
  paths.open(file.c_str());
  if(!paths.is_open()){
    printf("Error opening %s\n", file.c_str());
    return false;
  }

  SDSimulatorState state;
  littleDog.setSimUI(true);

  while(!paths.eof()){
    getline(paths,line);
    int index = line.find(" ");
    int foot = atoi((line.substr(0,index)).c_str());
    int index2 = line.find(" ",index+1);
    double x = atof((line.substr(index+1,index2-index-1)).c_str());
    double y = atof((line.substr(index2)).c_str());
   
    simulator->getState(&state);
    walkController.setStepPosition(foot, x, y);
    littleDog.runTrial(&state);
    simulator->getState(&state);
    simulator->setState(&state);  // This prevents the simulator straying from search - see handout faq (last page) for details
  }
  paths.close();
  return true;
}

//This method runs the program - Commented throughout
int main(int argc, char** argv){
  // Initialization
  SDSimulator *simulator;
  SDLittleDog littleDog;
  SDSlowWalk walkController;

  littleDog.setSimulated(true);
  littleDog.setController(&walkController);
  littleDog.setSimUI(false); //set it true if you want to see the simulation
  walkController.setLittleDog(&littleDog);
  
  littleDog.initializeSimulator();
  simulator = littleDog.getSimulator();

  if(argc > 1 && !strcmp(argv[1],"-path")){
    runPath("milestone.path", simulator, littleDog, walkController);
    return 0;
  }


////////////////////////////////////
///////////Classifiers////////////
////////////////////////////////////

// constants for classifiers
  int num_examples = 100;
  int num_feat = 19;

// generate training examples and store then in a text file
// SAVE THE EXISTING DATA FILES before rerunning this function. The current data files have 300 examples and take a while to generate, don't trash them...
  //makeFootPosClassifier(simulator, littleDog, walkController, num_examples, num_feat);

  printf("done, now making the classifier...\n");
    
//create classifiers
Classifier classif0(num_feat);
Classifier classif1(num_feat);
Classifier classif2(num_feat);
Classifier classif3(num_feat);

// Read data files and store in vector<Datum>
  vector<Datum> v0;
  classif0.readFile("classifier-data-dist0.txt", &v0, num_examples);
  vector<Datum> v1;
  classif1.readFile("classifier-data-dist1.txt", &v1, num_examples);
  vector<Datum> v2;
  classif2.readFile("classifier-data-dist2.txt", &v2, num_examples);
  vector<Datum> v3;
  classif3.readFile("classifier-data-dist3.txt", &v3, num_examples);


//train the classifiers
  classif0.train(v0);
  classif1.train(v1);
  classif2.train(v2);
  classif3.train(v3);
  classif0.printWeights();
  classif1.printWeights();
  classif2.printWeights();
  classif3.printWeights();

// store classifier weights in files
  classif0.writeWeightsToFile("Classifier0.txt");
  classif1.writeWeightsToFile("Classifier1.txt");
  classif2.writeWeightsToFile("Classifier2.txt");
  classif3.writeWeightsToFile("Classifier3.txt");

// load saved weights from files (if necessary)
  classif0.readWeightsFromFile("Classifier0.txt");
  classif1.readWeightsFromFile("Classifier1.txt");
  classif2.readWeightsFromFile("Classifier2.txt");
  classif3.readWeightsFromFile("Classifier3.txt");


//compute classifier errors : training and generalization
  double training_error = classif0.computeError(&v0,num_examples);
  printf("Training error 0 : %f\n", training_error);
  training_error = classif1.computeError(&v1,num_examples);
  printf("Training error 1 : %f\n", training_error);
  training_error = classif2.computeError(&v2,num_examples);
  printf("Training error 2 : %f\n", training_error);
  training_error = classif3.computeError(&v3,num_examples);
  printf("Training error 3 : %f\n", training_error);
  vector<Datum> v0_check, v1_check, v2_check, v3_check;
  classif0.readFile("classifier-data-dist0-train.txt", &v0_check, num_examples);
  classif1.readFile("classifier-data-dist1-train.txt", &v1_check, num_examples);
  classif2.readFile("classifier-data-dist2-train.txt", &v2_check, num_examples);
  classif3.readFile("classifier-data-dist3-train.txt", &v3_check, num_examples);

  double general_error = classif0.computeError(&v0_check,num_examples);
  printf("Generalization error 0 : %f\n", general_error);
general_error = classif1.computeError(&v1_check,num_examples);
  printf("Generalization error 1 : %f\n", general_error);
general_error = classif2.computeError(&v2_check,num_examples);
  printf("Generalization error 2 : %f\n", general_error);
general_error = classif3.computeError(&v3_check,num_examples);
  printf("Generalization error 3 : %f\n", general_error);



///////////////////////////////////////////   
/////////// A* algorithm /////////////////
//////////////////////////////////////////

//littleDog.setSimUI(true);

  // Create start node
  bduVec3f globalFeet[4];
  simulator->getGlobalFootPositions(globalFeet);
  Node nstart;
  nstart.foot_pos = globalFeet;
  nstart.moving_foot = 0;
  nstart.g = 0;
  nstart.center = FindNodeCenter(&nstart);
  nstart.h = 4*sqrt(pow(nstart.center[0]-GOALX,2) + pow(nstart.center[1]-GOALY,2))/d;
  nstart.f = nstart.g + nstart.h;
  nstart.parent = NULL;
  SDSimulatorState startState;
  simulator->getState(&startState);
  nstart.state = &startState;
  
  //Initialize queue, multiset, and other variables
  priority_queue<Node*, vector<Node*>, smallest_f> Q;
  stack<step> path;
  Q.push(&nstart);
  PrintFootPositions(&nstart);
  multiset<Node*, smallest_node> expanded;
  multiset<Node*, smallest_node>::iterator it;

  //actual A* algorithm loop structure

  time_t start, end;// timing variables
  time(&start);
  int numberFlunked = 0;
  while(!Q.empty()){
     Node* current = Q.top(); //Obtain next candidate for expansion
     Q.pop();
     it=expanded.find(current);
     if(it==expanded.end()){ //Check to see if not expanded before
        if(ValidStep(current, simulator, walkController, &numberFlunked, littleDog)){ //Check to see if feasible step
           if(GoalTest(current)){ //Check to see if goal state
              FindPath(&path, current);
              PrintFootPositions(current);
              break;
           }
           expanded.insert(current); //mark as expanded node
           for(int i = -2; i < 3; i++){ //Generate children deamed feasible by the classifiers
              for(int j = -2; j < 3; j++){
                 if(i != 0 && j != 0){
                    Node* child = GenerateChildNode(simulator, &classif0, &classif1, &classif2, &classif3, current, i, j);
                    if(child != NULL){
                       Q.push(child);
                    }
                 }
              }  
           }     
        }
     }else printf("-----REPEATED STATE\n");
  } 
  time(&end);
  printf("RUN TIME: %f\n", difftime(end, start));
  
  printf("Nodes Flunked = %d, Final Queue Size = %d, Nodes Expanded = %d\n", numberFlunked, Q.size(), expanded.size());


  //Save the path (stored in a struct) to a path file
  if(path.empty()){
     printf("Path not found\n");
  }else{
     SavePathToFile(&path);
  }
  
  //Delete all nodes and classifier (so we aren't orphaning memory)
  for(multiset<Node*, smallest_node>::iterator it = expanded.begin();it != expanded.end() && *it != &nstart; it++){
     delete *it;
  }
  expanded.clear();
  if(!Q.empty()){
     for(Node* next = Q.top(); !Q.empty(); next = Q.top()){
        Q.pop();
        delete next;
     }
  }
  
  //Exit program

  return 0;
} 
